#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav_msg_pkg/action/progress.hpp"

using nav_msg_pkg::action::Progress;

class ProgressActionClient : public rclcpp::Node
{
public:
    ProgressActionClient() : Node("progress_action_client")
    {
        RCLCPP_INFO(this->get_logger(), "Progress action client started");
        // 创建客户端
        progress_action_client_ = rclcpp_action::create_client<Progress>(
            this,
            "get_sum");
        // 发送请求
        // 处理目标值的服务端响应
        // 处理连续反馈
        // 处理最终响应
    }

    bool connect_server()
    {
        while (!progress_action_client_->wait_for_action_server(std::chrono::seconds(1)))
        {
            // 对 ctrl + c 退出程序
            if (!rclcpp::ok())
            {
                RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "退出！");
                return false;
            }
            RCLCPP_INFO(this->get_logger(), "waiting for server...");
        }
        return true;
    }

    void send_goal(int num)
    {
        auto goal = Progress::Goal();
        goal.num = num;
        auto options = rclcpp_action::Client<Progress>::SendGoalOptions();
        options.goal_response_callback = std::bind(&ProgressActionClient::goal_response_callback, this, std::placeholders::_1);
        options.feedback_callback = std::bind(&ProgressActionClient::feedback_callback, this, std::placeholders::_1, std::placeholders::_2);
        options.result_callback = std::bind(&ProgressActionClient::result_callback, this, std::placeholders::_1);
        std::shared_future<rclcpp_action::ClientGoalHandle<Progress>::SharedPtr> future = progress_action_client_->async_send_goal(goal, options);
        RCLCPP_INFO(this->get_logger(), "发送目标值 %d", num);
    }

private:
    rclcpp_action::Client<Progress>::SharedPtr progress_action_client_;

    void goal_response_callback(rclcpp_action::ClientGoalHandle<Progress>::SharedPtr goal_handle)
    {
        if (!goal_handle)
        {
            RCLCPP_INFO(this->get_logger(), "目标被拒绝");
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "目标被接受");
        }
    }

    void feedback_callback(
        rclcpp_action::ClientGoalHandle<Progress>::SharedPtr,
        const std::shared_ptr<const Progress::Feedback> feedback)
    {
        RCLCPP_INFO(this->get_logger(), "当前进度：%f", feedback->progress);
    }

    void result_callback(const rclcpp_action::ClientGoalHandle<Progress>::WrappedResult &result)
    {
        switch (result.code)
        {
        case rclcpp_action::ResultCode::SUCCEEDED:
            RCLCPP_INFO(this->get_logger(), "Goal succeeded! sum=%d", result.result->sum);
            break;
        case rclcpp_action::ResultCode::ABORTED:
            RCLCPP_ERROR(this->get_logger(), "Goal was aborted,当前sum=%d", result.result->sum);
            break;
        case rclcpp_action::ResultCode::CANCELED:
            RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
            break;
        default:
            RCLCPP_ERROR(this->get_logger(), "Unknown result code");
            break;
        }
    }
};

int main(int argc, char *argv[])
{
    if (argc != 2)
    {
        RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "请提交一个int类型的参数");
        return 1;
    }
    rclcpp::init(argc, argv);
    auto node = std::make_shared<ProgressActionClient>();
    if (!node->connect_server())
    {
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "服务未启动!");
        return 0;
    }
    node->send_goal(atoi(argv[1]));
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}